#ifndef _motor_h

void encoder_init(void);
void motor_init(void);
void read_encoder(void);

#define MOTOR_LF_DIR               (D2 )
#define MOTOR_LF_PWM               (PWM1_MODULE3_CHA_D0)

#define MOTOR_RF_DIR               (D3 )
#define MOTOR_RF_PWM               (PWM1_MODULE3_CHB_D1)

#define MOTOR_LB_DIR               (D14 )
#define MOTOR_LB_PWM               (PWM1_MODULE0_CHA_D12)

#define MOTOR_RB_DIR               (D15 )
#define MOTOR_RB_PWM               (PWM1_MODULE0_CHB_D13)

#define ENCODER_1                       (QTIMER1_ENCODER1)
#define ENCODER_1_LSB                   (QTIMER1_ENCODER1_CH1_C0)
#define ENCODER_1_DIR                   (QTIMER1_ENCODER1_CH2_C1)

#define ENCODER_2                       (QTIMER1_ENCODER2)
#define ENCODER_2_LSB                   (QTIMER1_ENCODER2_CH1_C2)
#define ENCODER_2_DIR                   (QTIMER1_ENCODER2_CH2_C24)

#define ENCODER_3                       (QTIMER2_ENCODER1)
#define ENCODER_3_LSB                   (QTIMER2_ENCODER1_CH1_C3)
#define ENCODER_3_DIR                   (QTIMER2_ENCODER1_CH2_C25)

#define ENCODER_4                       (QTIMER3_ENCODER2)
#define ENCODER_4_LSB                   (QTIMER3_ENCODER2_CH1_B18)
#define ENCODER_4_DIR                   (QTIMER3_ENCODER2_CH2_B19)


typedef enum
{	
	MOTOR_LF ,
	MOTOR_RF ,
	MOTOR_LB ,
	MOTOR_RB 
//	int16 encoder;
//	int16 last_encoder;
//	int16 target_speed;
//	int16 target_speed_last;
//	int32 distance;
//	
//	float error;
//	float error_p;
//	float kp;
//	float ki;
//	float k_response;
//	float k_restraint;
//	
//	int16 duty;

}Motor_num;

#define gpio_lb_ahead GPIO_HIGH
#define gpio_lb_back GPIO_LOW

#define gpio_lf_ahead GPIO_HIGH
#define gpio_lf_back GPIO_LOW

#define gpio_rf_ahead GPIO_LOW
#define gpio_rf_back GPIO_HIGH

#define gpio_rb_ahead GPIO_LOW
#define gpio_rb_back GPIO_HIGH

typedef struct {
    float gyro_x;
    float gyro_y;
    float gyro_z;
    float acc_x;
    float acc_y;
    float acc_z;
} icm_param_t;

typedef struct
{
	float gyro_z_final;
	float gyro_z_last;
	float angle;
//	float angle2;
} gyro_yaw_t;
extern icm_param_t icm_data,icm_data_t;

extern int16 encoder[4];
extern int16 p_encodr[4];
extern float pid_motor[4];
extern float target_motor[4];
extern gyro_yaw_t Yaw;

extern float Velocity_KP[4];
extern float Velocity_KI[4];

void moto_contorl(void);
void mailun_jiesuan (int Vx,int Vy,int Vz);
float Turn_PD(float Tar_angle_Z);

void Incremental_PI();


extern uint8_t clear;
void encoder_setoff();
void while_delay(uint32_t time);
#endif // !_motor_h

